namespace Microsoft.Robotics.Core.Algorithms
{
    using System;
    using System.Collections.Generic;
    using System.Text;
    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// Fits a  line to a set of x's and y's using discrete linear approximation (Least Square)    
    /// </summary>
    public class PolynomialEstimator    
    {
        /// <summary>
        /// Initializes a new instance of the <see cref="PolynomialEstimator" /> class.
        /// </summary>
        public PolynomialEstimator() 
        {   
        }

        /// <summary>
        /// Gets or sets data points
        /// </summary>
        public List<FastTuple<double, double>> DataPoints { get; set; }

        /// <summary>
        /// Estimates a value of a function given a 1st degree fit and a measurement
        /// </summary>
        /// <param name="fit">Fitted polynomial for which to estimate a value</param>
        /// <param name="measurement">Measurement for which to estimate a value</param>
        /// <returns>Estimated function value</returns>
        public static double Estimate(FastTuple<double, double> fit, double measurement)
        {
            return fit.Item1 + fit.Item2 * measurement;
        }

        /// <summary>
        /// Estimates a value of a function given a 2nd degree fit and a measurement
        /// </summary>
        /// <param name="fit">Fitted polynomial for which to estimate a value</param>
        /// <param name="measurement">Measurement for which to estimate a value</param>
        /// <returns>Estimated function value</returns>
        public static double Estimate(FastTuple<double, double, double> fit, double measurement)
        {
            return fit.Item1 + fit.Item2 * measurement + fit.Item3 * measurement * measurement;
        }

        /// <summary>
        /// Add a measurement
        /// </summary>
        /// <param name="x">X values</param>
        /// <param name="y">Y values</param>
        public void Add(double x, double y) 
        {
            if (null == this.DataPoints)
            {
                this.DataPoints = new List<FastTuple<double, double>>();
            }

            FastTuple<double, double> t = new FastTuple<double, double>(x, y);

            this.DataPoints.Add(t);
        }

        /// <summary>
        /// Clears all data points
        /// </summary>
        public void Clear()
        {
            if (null != this.DataPoints)
            {
                this.DataPoints.Clear();
            }
        }

        /// <summary>
        /// Fits a parabola to a previously added sample set. 
        /// </summary>
        /// <param name="outFit">Tuple containing coefficients in the y = A*X^2 + BX + C form, where first element of the tuple is 
        /// C (0'th order), second is B (1st order) and last is A (2nd order).</param>
        /// <returns>Whether or not fitting succeeded.</returns>
        public bool FitAParabola(out FastTuple<double, double, double> outFit)
        {
            if (null == this.DataPoints)
            {
                throw new InvalidOperationException("Collection not initialized");
            }

            outFit = new FastTuple<double, double, double>();

            if (this.DataPoints.Count < 3)
            {
                return false;
            }

            double sumXY = 0;
            double sumX = 0;
            double sumY = 0;
            double sumXSquared = 0;
            double sumYXSquared = 0;
            double sumXCubed = 0;
            double sumXFourth = 0;

            int length = this.DataPoints.Count;

            for (int i = 0; i < length; ++i)
            {
                double x = this.DataPoints[i].Item1;
                double y = this.DataPoints[i].Item2;

                double xsquared = x * x;

                sumXY += x * y;
                sumX += x;
                sumY += y;
                sumXSquared += xsquared;
                sumXCubed += xsquared * x;
                sumYXSquared += xsquared * y;
                sumXFourth += xsquared * xsquared;
            }

            if (0 == sumXFourth)
            {
                return false;
            }

            double commonElem1 = (sumXCubed * sumXCubed) / sumXFourth;
            double commonDenom = sumXSquared - commonElem1;

            if (0 == commonDenom) 
            {
                return false;
            }

            double commonElem2 = (sumXCubed * sumYXSquared) / sumXFourth;
            double tempA = sumXY - commonElem2;
            tempA /= commonDenom;

            double commonElem3 = (sumXCubed * sumXSquared) / sumXFourth;
            double tempB = sumX - commonElem3;
            tempB /= commonDenom;

            double tempC = length - (sumX - commonElem3) / (sumXSquared - commonElem1) * sumX;
            tempC -= sumXSquared * sumXSquared / sumXFourth;
            tempC += tempB * commonElem3;

            if (0 == tempC)
            {
                return false;
            }

            outFit.Item1 = sumY - sumX * tempA - (sumXSquared / sumXFourth) * sumYXSquared + tempA * commonElem3;
            outFit.Item1 /= tempC;

            outFit.Item2 = tempA - outFit.Item1 * tempB;

            outFit.Item3 = (sumYXSquared - outFit.Item1 * sumXSquared - outFit.Item2 * sumXCubed) / sumXFourth;

            return true;
        }

        /// <summary>
        /// Fits a line to a previously added sample set. 
        /// </summary>
        /// <param name="outFit">Tuple containing coefficients in the y = A*X + B form, where first element of the tuple is 
        /// B (0'th order), and the second is A (1st order)</param>
        /// <returns>Whether or not fitting succeeded.</returns>
        public bool FitALine(out FastTuple<double, double> outFit)
        {
            if (null == this.DataPoints)
            {
                throw new InvalidOperationException("Collection not initialized");
            }

            outFit = new FastTuple<double, double>();

            if (this.DataPoints.Count < 2)
            {
                return false;
            }

            double sumXY = 0;
            double sumX = 0;
            double sumY = 0;
            double sumXSquared = 0;

            int length = this.DataPoints.Count;

            for (int i = 0; i < length; ++i)
            {
                double x = this.DataPoints[i].Item1;
                double y = this.DataPoints[i].Item2;

                sumXY += x * y;
                sumX += x;
                sumY += y;
                sumXSquared += x * x;
            }

            double divisor = length * sumXSquared - sumX * sumX;

            if (divisor != 0)
            {
                outFit.Item1 = (sumXSquared * sumY - sumXY * sumX) / divisor; // A
                outFit.Item2 = (length * sumXY - sumX * sumY) / divisor; // B

                return true;
            }
            else
            {
                return false;
            }
        }
    }
}
